#!/usr/bin/python
 
import cv
import time
import Image
import os
from socket import socket, AF_INET, SOCK_DGRAM

from twisted.internet import reactor

from http_mjpeg_client import MJPEGFactory

import cStringIO # *much* faster than StringIO
import urllib
import Image




port = 9930
hostname = '192.168.1.230'

xpos = 0;
xoldpos = 0;
ypos = 0; 
faceDetected = 0
distance = 0
hxpos = 0
hypos = -20



detecttime=time.localtime()
leavtime=detecttime
 
def DetectFace(image):

    global detecttime
    global leavtime

    global eyesCascade
    global faceCascade
	 
    min_size = (40,40)
    image_scale = 2
    haar_scale = 1.2
    min_neighbors = 4
    haar_flags = 0
 
    # Allocate the temporary images
    grayscale = cv.CreateImage((image.width, image.height), 8, 1)
    smallImage = cv.CreateImage(
            (
                cv.Round(image.width / image_scale),
                cv.Round(image.height / image_scale)
            ), 8 ,1)
 
    # Convert color input image to grayscale
    cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY)
 
    # Scale input image for faster processing
    cv.Resize(grayscale, smallImage, cv.CV_INTER_LINEAR)
 
    # Equalize the histogram
    cv.EqualizeHist(smallImage, smallImage)
 
    # Detect the faces
    faces = cv.HaarDetectObjects(smallImage, faceCascade, cv.CreateMemStorage(0), haar_scale, min_neighbors, haar_flags, min_size  )

    global xpos
    global ypos
    global faceDetected

    global distance

    if faces:

	faceDetected=1 
        for ((x, y, w, h), n) in faces:
            # the input to cv.HaarDetectObjects was resized, so scale the
            # bounding box of each face and convert it to two CvPoints
            pt1 = (int(x * image_scale), int(y * image_scale))
            pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))

            pt11 = (int((x + w/2) * image_scale), int((y + h/2) * image_scale))
            pt22 = (int((x + w/2) * image_scale), int((y + h/2) * image_scale))

            cv.Rectangle(image, pt1, pt2, cv.RGB(255, 0, 0), 3, 4, 0)
            cv.Rectangle(image, pt11, pt22, cv.RGB(0, 255, 0), 3, 4, 0)

	    xpos = int(x + w/2)
	    ypos = int(y + h/2)

	    distance = w

    else:

	faceDetected = 0
	
    return image
 
#----------
# M A I N
#----------
 
detected = 0;

faceCascade = cv.Load("haarcascade_frontalface_alt.xml")

udp = socket(AF_INET,SOCK_DGRAM)
udp.sendto("ip", (hostname, port))


moving = 0

direction = "none"

while (cv.WaitKey(15)==-1):
    
    global xpos
    global xoldpos
    global ypos
    global yoldpos
    global distance
    global hxpos
    global hypos
    global moving
    global direction

    file =  urllib.urlopen('http://192.168.1.2:8080/shot.jpg')
    im = cStringIO.StringIO(file.read()) # constructs a StringIO holding the image
    img = Image.open(im)
    img.save('save.jpg')

    imgc=cv.LoadImage('save.jpg', cv.CV_LOAD_IMAGE_COLOR)


    image = DetectFace(imgc)
    cv.ShowImage("face detection test", image)
        
    x = xoldpos - xpos
    
    xcenter=150
    ycenter=130
    

    print "x:%d  y:%d D: %d  hxpos: %d moving: %d direction : %s" % ( xpos, ypos,distance,hxpos,moving,direction )

    
    if faceDetected == 1 :
	    

	    if distance > 70 and  distance < 110 :

		if moving == 1:
			udp.sendto("stop", (hostname, port))
		moving = 0
		direction = "none"

		if xpos > xcenter+10:
			udp.sendto("hl", (hostname, port))
			hxpos=hxpos+2
		if xpos < xcenter-10:
			udp.sendto("hr", (hostname, port))
			hxpos=hxpos-2
    
		if ypos > ycenter+10:
		        udp.sendto("hd", (hostname, port))
			hypos=hypos+2
		if ypos < ycenter-10:
		        udp.sendto("hu", (hostname, port))
			hypos=hypos-2


	    else:
	   
		moving =1
		
		if distance > 110 and direction != "back" :
			udp.sendto("mb", (hostname, port))
			direction = "back"

		if hxpos > 5 or hxpos < -5 :
			
			direction = "none"
 
			if hxpos > 5:
				udp.sendto("hr", (hostname, port))
				udp.sendto("hr", (hostname, port))
				hxpos=hxpos-4
				udp.sendto("tr", (hostname, port))
		    	if hxpos < -5:
				udp.sendto("hl", (hostname, port))
				udp.sendto("hl", (hostname, port))
				hxpos=hxpos+4
				udp.sendto("tl", (hostname, port))
	
		else:
	
			if distance < 70 and direction != "forward":
				udp.sendto("mf", (hostname, port))
				direction="forward"


    else:
		print "not detected"

		if moving == 1:
			udp.sendto("stop", (hostname, port))
			moving = 0
			direction = "none"
		
